#include "main.h"

st_Invertor g_invertorParam = {  // 逆变器所有参数
    .adc_offset_flag = 0,   // 初始值表示ADC零偏还未初始化
};           
st_MotorCtrlParam g_MotorCtrlParam;     // 电机控制的所有参数
st_CovModStrategy CovModStrategy;
st_MCStrategy MCStrategy;

RuntimeCounter runtime = {0};
uint16_t err_status_1;
uint16_t err_status_2;
uint16_t control_reg;

extern DRV8353_Handle drv8353_handle;
extern float32_t Me_ThetaOffset;

float sine_table[TRIG_TABLE_SIZE];
uint8_t table_initialized;

float sin_val;
float cos_val;

extern void SVPWM(void);
extern void AlgorithmInit(void);
extern void CurrentLoop(void);
extern void SpeedObserver(void);
extern void SpeedLoop(void);
extern void PositionLoop(void);
extern void Motor_DC_Alignment(void);
float data = 0;
float data1 = 0;

extern uint32_t time1, time2;
uint32_t time3, time4;
uint32_t cost_time1, cost_time2;
st_MachineState controlState = Machine_Init;
extern DRV8353_Obj drv8353_obj1;

uint16_t fault1_reg = 0;
uint16_t fault2_reg = 0;

extern float observer_wt;
extern float observer_wm;
extern float actual_count;
extern float actual_theta_m;
extern float actual_theta_e;
extern float actual_speed;
extern float acc_w;

extern float kp_out;
extern float ki_out;
extern float kd_out;

extern float pos_Pout;
extern float pos_Iout;
extern float pos_Dout;

extern int16_t QEP_Count_Sum;
extern int16_t QEP_Count_delta;
void StateMachine(void)
{
  switch(controlState)
  {
    case Machine_Init:
    {
        if (g_invertorParam.adc_offset_flag)
        {
            // 检测完ADC零偏值后，转到停止状态
            StateSwitch(Machine_STOP);
            AlgorithmInit();
        }
    }
    break;

    case Machine_STOP:
    {
        // 停止发波
        HAL_TIM_PWM_Stop(&htim8, TIM_CHANNEL_1);
        HAL_TIMEx_PWMN_Stop(&htim8, TIM_CHANNEL_1);
        HAL_TIM_PWM_Stop(&htim8, TIM_CHANNEL_2);
        HAL_TIMEx_PWMN_Stop(&htim8, TIM_CHANNEL_2);
        HAL_TIM_PWM_Stop(&htim8, TIM_CHANNEL_3);
        HAL_TIMEx_PWMN_Stop(&htim8, TIM_CHANNEL_3);
        // DRV8353_cutDown();
        // Tamagawa_SendCmd(GET_ALL);
        SpeedObserver();
    }
    break;

    case Machine_RUN_INIT:
    {
        AlgorithmInit();
        DRV8353_wakeUp();
        StateSwitch(Machine_RUN);

       __HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_1, 0);
       __HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_2, 0);
       __HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_3, 0);

       HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_1);       // A相输出
       HAL_TIMEx_PWMN_Start(&htim8, TIM_CHANNEL_1);  
       HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_2);
       HAL_TIMEx_PWMN_Start(&htim8, TIM_CHANNEL_2);    // B相输出
       HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_3);
       HAL_TIMEx_PWMN_Start(&htim8, TIM_CHANNEL_3);    // C相输出

    }
    break;

    case Machine_Prealignment:
    {
        Motor_DC_Alignment();
        Tamagawa_SendCmd(GET_ALL);
        Me_ThetaOffset = tg_comm.encoder_theta;
    }
    break;


    case Machine_RUN:
    {
        /* 本工程中SysTick 使用HCLK作为时钟源，频率为180Mhz */
        HAL_GPIO_WritePin(GPIOA, GPIO_PIN_15, GPIO_PIN_SET);
        time1 = SysTick->VAL;
        // Tamagawa_SendCmd(GET_ALL);

        SpeedObserver();

        // PositionLoop();

        SpeedLoop();

        CurrentLoop();

        SVPWM();

        addDataToChannel(MCStrategy.Iq_Set, Vofa_Channel_0);
        addDataToChannel(MCStrategy.Iq_Feedback, Vofa_Channel_1);
        addDataToChannel(observer_wm * 9.5492965855137201461330258023509f, Vofa_Channel_2);
        addDataToChannel(MCStrategy.Speed_Set, Vofa_Channel_3);
        addDataToChannel(MCStrategy.Position_Set, Vofa_Channel_4);
        addDataToChannel(MCStrategy.Position_Feedback, Vofa_Channel_5);
        addDataToChannel(MCStrategy.Torque_Set, Vofa_Channel_6);
        addDataToChannel(ki_out, Vofa_Channel_7);
        addDataToChannel(kd_out, Vofa_Channel_8);
        addDataToChannel(MCStrategy.Speed_Observer_h3_sum, Vofa_Channel_9);
        
        time2 = SysTick->VAL;
        if (time1 > time2)
        {
            cost_time1 = time1 - time2;
        }
        else
        {
            cost_time1 = SysTick->LOAD - time2 + time1;
        }
        HAL_GPIO_WritePin(GPIOA, GPIO_PIN_15, GPIO_PIN_RESET);
    }
    break;

    case Machine_FAULT:
    {
        HAL_TIM_PWM_Stop(&htim8, TIM_CHANNEL_1);
        HAL_TIMEx_PWMN_Stop(&htim8, TIM_CHANNEL_1);
        HAL_TIM_PWM_Stop(&htim8, TIM_CHANNEL_2);
        HAL_TIMEx_PWMN_Stop(&htim8, TIM_CHANNEL_2);
        HAL_TIM_PWM_Stop(&htim8, TIM_CHANNEL_3);
        HAL_TIMEx_PWMN_Stop(&htim8, TIM_CHANNEL_3);

        fault1_reg = DRV8353_readSpi(&drv8353_obj1, Address_Status_0);
        fault2_reg = DRV8353_readSpi(&drv8353_obj1, Address_Status_1);

        // DRV8353_cutDown();
        while(1);
    }
    break;

    default:
    {

    }
    break;
  }
}

void StateSwitch(st_MachineState state)
{
    controlState = state;
}


void RuntimeCounter_Init(void) {
    memset((void*)&runtime, 0, sizeof(runtime));
}

void RuntimeCounter_Update(void) {
    // 在SysTick中断中调用（每1ms调用一次）
    runtime.ms_count++;
    
    if (runtime.ms_count >= 1000) {
        runtime.ms_count = 0;
        runtime.seconds++;
        
        if (runtime.seconds >= 60) {
            runtime.seconds = 0;
            runtime.minutes++;
            
            if (runtime.minutes >= 60) {
                runtime.minutes = 0;
                runtime.hours++;
                
                if (runtime.hours >= 24) {
                    runtime.hours = 0;
                    runtime.days++;  // 天数无上限
                }
            }
        }
    }
}

void RuntimeCounter_GetString(char* buffer, uint32_t size) {
    // 复制到临时变量防止中断修改导致数据不一致
    RuntimeCounter temp;
    
    temp = runtime;
    
    
    // 格式化输出：0d 0h 0m 0s
    if (temp.days > 0) {
        snprintf(buffer, size, "%lud %luh %lum %lus", 
                 temp.days, temp.hours, temp.minutes, temp.seconds);
    } else if (temp.hours > 0) {
        snprintf(buffer, size, "%luh %lum %lus", 
                 temp.hours, temp.minutes, temp.seconds);
    } else {
        snprintf(buffer, size, "%lum %lus", 
                 temp.minutes, temp.seconds);
    }
}

// 电流大小控制（通过PWM占空比调节，0.0f-1.0f应0-100%）
#define ALIGN_CURRENT_DUTY      0.05f

/**
 * @brief  直流预定位：将转子锁定在电角度0位置
 * @param  duration_ms：定位持续时间（毫秒）
 */
void Motor_DC_Alignment(void) 
{
    DRV8353_wakeUp();
    
    uint32_t PWM_Period = TIM8->ARR;

    // 1. 控制A相通正向电流（上桥臂开，下桥臂关）
    __HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_1, (uint32_t)(PWM_Period * ALIGN_CURRENT_DUTY));
    
    // 2. B相和C相下桥臂开，上桥臂关（形成回路，电流较小）
    __HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_2, 0);
    __HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_3, 0);
  
    // 3. 输出PWM控制电流大小（通过占空比调节）
    HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_1);       // A相输出
    HAL_TIMEx_PWMN_Start(&htim8, TIM_CHANNEL_1);  
    HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_2);
    HAL_TIMEx_PWMN_Start(&htim8, TIM_CHANNEL_2);    // B相输出
    HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_3);
    HAL_TIMEx_PWMN_Start(&htim8, TIM_CHANNEL_3);    // C相输出
  
    // 4. 保持电流，等待转子稳定
    if(CovModStrategy.nTime > 1) //电机启动+匀速时间是1s
    {
        // 5. 定位完成，关闭PWM（避免过热）
        StateSwitch(Machine_STOP);
    }
  
}
